#include "utils.hpp"
#include <iostream>

#define pi 3.1415

using namespace sf;

Vector3f prod(Vector3f a, Vector3f b) {
  Vector3f v;
  v.x = a.y * b.z - a.z * b.y;
  v.y = a.z * b.x - a.x * b.z;
  v.z = a.x * b.y - a.y * b.x;
  return v;
}

float norm(Vector3f v) { return v.x * v.x + v.y * v.y + v.z * v.z;};



float getVectorAngle(Vector3f v) {
  /** determine the angle between v and Vector3f(0.f,1.f,0.f) in
      the z=0 plane */
  float norm = sqrt(v.x * v.x + v.y * v.y);
  //float ac = acos(v.y / norm);
  //float as = asin(v.x / norm);
  float angle = acos(abs(v.x) / norm);// abs(ac);
  if (v.x >= 0 and v.y >= 0) return angle;
  if (v.x >= 0 and v.y <= 0) return 2 * pi -angle;
  if (v.x <= 0 and v.y >= 0) return pi - angle;
  if (v.x <= 0 and v.y <= 0) return angle + pi;
  return 0.f;
}

float getVectorAngle(glm::vec3 v) {
  /** determine the angle between v and Vector3f(0.f,1.f,0.f) in
      the z=0 plane */
  float norm = sqrt(v.x * v.x + v.y * v.y);
  //float ac = acos(v.y / norm);
  //float as = asin(v.x / norm);
  float angle = acos(abs(v.x) / norm);// abs(ac);
  if (v.x >= 0 and v.y >= 0) return angle;
  if (v.x >= 0 and v.y <= 0) return 2 * pi -angle;
  if (v.x <= 0 and v.y >= 0) return pi - angle;
  if (v.x <= 0 and v.y <= 0) return angle + pi;
  return 0.f;
}
//int mod(int i, int m) { 
//  if (i >= 0) return (i % m); else return - ((-i) % m) + m;}; 
